#include "CTorqueControl.h"

CML_NAMESPACE_USE();

CTorqueControl::CTorqueControl(void)
{
	f_control.open("control_record.txt",ios::trunc);
	f_measure.open("measure_record.txt",ios::trunc);
	outputLimit = 5;
}

CTorqueControl::~CTorqueControl(void)
{
	
}

const Error *CTorqueControl::EnableAction( void )
{
	const Error *err=sa->SetAmpMode(AMPMODE_CAN_VELOCITY);
	CAction::EnableAction();
	return err;
}

void CTorqueControl::DoAct(void)
{
	delta[0] = sa->GetForceSet() - sa->GetForceMeasure();
	delta[1] = sa->GetVelForceSet() - sa->GetVelForceMeasure();
	output = (sa->GetVelForceSet()) / (sa->GetElasticCoefficient()) + Kp * delta[0] + Kd * delta[1]-sa->GetVelLoad();
	if(output > outputLimit)
		output = outputLimit;
	else if(output<-outputLimit)
		output=-outputLimit;

	sa->SetTargetVel(output * 120000 / PI);
	
	sa->GetVelocityActual(velMotorActual);

	f_control<<(sa->GetForceMeasure())<<"  ";
	f_control<<(sa->GetForceSet())<<"  ";
	f_control<<(Kd * delta[1])<<"  ";
	f_control<<(Kp * delta[0])<<"  ";
	f_control<<output<<"  ";
	f_control<<(velMotorActual/240000*PI*2)<<endl;
	
	f_measure<<sa->GetPosSpring()<<"  ";
	f_measure<<sa->GetVelSpring()<<"  ";
	f_measure<<sa->GetAccSpring()<<"  ";
	
	f_measure<<sa->GetPosLoad()<<"  ";
	f_measure<<sa->GetVelLoad()<<"  ";
	f_measure<<sa->GetAccLoad()<<"  ";

	f_measure<<sa->GetPosMotor()<<"  ";
	f_measure<<sa->GetVelMotor()<<"  ";
	f_measure<<sa->GetAccMotor()<<endl;

	return;
}
